/*-----------------------------------------------------------------------------
 *FILE:			UPI_OS.h
 *VER:			V0.3
 *DESCRIPTION:	UPI OS Interface implemention on Linux platform
 *
 *NOTE:			Name			Version			Date			Modification
				----------		-------			----------		-------------
				Zhao Liang		V0.1			2008.04.03		Create
				Zhao Liang		V0.3			2008.04.16		Alpha Test
 *
-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
	Standard include files:
-----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
#include <errno.h>

/*-----------------------------------------------------------------------------
	Project include files:
-----------------------------------------------------------------------------*/
#include "UPI_OS.h"
#include "UPI_OS_private.h"
#include <semaphore.h>
#include <time.h>
#include <sys/time.h>
#include <sys/times.h>
//#include <sys/msg.h>
#include <linux/msg.h>

#include <pthread.h>

#include <unistd.h>

#include <android/log.h>
#define LOG_TAG "UPI_OS"
#define LOGI(...) //__android_log_print(ANDROID_LOG_INFO  , LOG_TAG, __VA_ARGS__)



/*-----------------------------------------------------------------------------
	Types and defines:
-----------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------------
	Globle data:
-----------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------------
	Private data:
-----------------------------------------------------------------------------*/
static UPI_OsTimerInfo __Timer[C_UPI_OS_MAX_TIMER_COUNT];
static UPI_OsTimerHeader __OsTimer = {	0,
										(UPI_OsSemHandle)C_UPI_OS_INVALID_SEMA_HANDLE,
										(UPI_OsQueueHandle)C_UPI_OS_INVALID_QUEUE_HANDLE,
										C_UPI_OS_INVALID_THREAD_HANDLE,
										C_UPI_OS_INVALID_THREAD_HANDLE,
										__Timer};

/*-----------------------------------------------------------------------------
	Internal Prototypes:
-----------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------------
	Functions
-----------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_get_version
 * Description	: Get OS version string
 * Input    	: none
 * Output   	: none
 * Return   	: pointer to OS version string
 * ----------------------------------------------------------------------*/
UPI_int8 *UPI_os_get_version(void)
{
	return C_UPI_OS_VERSION;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_printf
 * Description	: output formated string
 * Input    	: fmt	string format
 				  ...	data list
 * Output   	: none
 * Return   	: number of char outputed
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_printf(UPI_int8 *fmt, ...)
{
	va_list argptr;
  	int cnt;
  	char msg[1024];
  	
  	va_start(argptr, fmt);
  	cnt = vsprintf(msg,fmt, argptr);
  	va_end(argptr);
//	LOGI(msg);

	return 0;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_warn_printf
 * Description	: output yellow color formated string
 * Input    	: fmt	string format
 				  ...	data list
 * Output   	: none
 * Return   	: number of char outputed
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_warn_printf(UPI_int8 *fmt, ...)
{
	UPI_int32 rc;
	va_list args;

	va_start(args, fmt);
//	printf("\033[0;43;38m");
	rc = vprintf(fmt, args);
//	printf("\033[0m");
	va_end(args);

	return rc;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_error_printf
 * Description	: output red color formated string
 * Input    	: fmt	string format
 				  ...	data list
 * Output   	: none
 * Return   	: number of char outputed
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_error_printf(UPI_int8 *fmt, ...)
{
	UPI_int32 rc;
	va_list args;

	va_start(args, fmt);
//	printf("\033[0;41;38m");
	rc = vprintf(fmt, args);
//	printf("\033[0m");
	va_end(args);

	return rc;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_set_time
 * Description	: set os time
 * Input    	: none
 * Output   	: pointer to UPI_OsTime struct
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_set_time(pUPI_OsTime pTime)
{
	struct tm localTime;
	struct timeval	curtime;
	struct timezone	tz;

	if(pTime == NULL)
	{
		UPI_os_error_printf("UPI_os_set_time() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	localTime.tm_year	= pTime->Year - 1900;
	localTime.tm_mon	= pTime->Month - 1;
	localTime.tm_mday	= pTime->Day;
	localTime.tm_hour	= pTime->Hour;
	localTime.tm_min	= pTime->Minute;
	localTime.tm_sec	= pTime->Second;

	curtime.tv_sec		= 	mktime(&localTime);
	curtime.tv_usec		=	0;
	tz.tz_minuteswest	=	0 - (pTime->TimeZone)*60;

	settimeofday(&curtime, &tz);

	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_get_time
 * Description	: get os time
 * Input    	: none
 * Output   	: pTime pointer to UPI_OsTime struct
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_get_time(pUPI_OsTime pTime)
{
	UPI_int32		ErrorCode;
	struct timeval	curtime;
	struct timezone	tz;
	struct tm 		*tms;

	if(pTime == NULL)
	{
		UPI_os_error_printf("UPI_os_get_time() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	ErrorCode = gettimeofday(&curtime, &tz);
	if(ErrorCode == -1)
	{
		UPI_os_error_printf("UPI_os_get_time() error\n");
		return E_UPI_OS_TIME_GET_ERROR;
	}

	tms = localtime(&(curtime.tv_sec));

	pTime->Year		= tms->tm_year + 1900;
	pTime->Month	= tms->tm_mon+1;
	pTime->Day		= tms->tm_mday;
	pTime->Hour		= tms->tm_hour;
	pTime->Minute	= tms->tm_min;
	pTime->Second	= tms->tm_sec;
	pTime->WeekDay	= tms->tm_wday;
	pTime->TimeZone = 0 - tz.tz_minuteswest/60;
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_time_local2gmt
 * Description	: convert local time to GMT time
 * Input    	: pTime	pointer to UPI_OsTime struct
 * Output   	: none
 * Return   	: GMT time
 * ----------------------------------------------------------------------*/
UPI_uint32 UPI_os_time_local2gmt(pUPI_OsTime	pTime)
{
	struct tm localTime;

	if(pTime == NULL)
	{
		UPI_os_error_printf("UPI_os_time_local2gmt() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	memset(&localTime, 0, sizeof(struct tm));

	localTime.tm_year	= pTime->Year-1900;
	localTime.tm_mon	= pTime->Month-1;
	localTime.tm_mday	= pTime->Day;
	localTime.tm_hour	= pTime->Hour;
	localTime.tm_min	= pTime->Minute;
	localTime.tm_sec	= pTime->Second;

	return (UPI_uint32)mktime(&localTime);

}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_time_gmt2local
 * Description	: convert GMT time to local time
 * Input    	: pGMTTime	pointer to GMT time
 * Output   	: pTime		pointer to local time
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_time_gmt2local(UPI_int32 *pGMTTime, pUPI_OsTime pTime)
{
	struct tm *plocalTime;

	if(pTime == NULL || pGMTTime == NULL)
	{
		UPI_os_error_printf("UPI_os_time_gmt2local() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	plocalTime = localtime((time_t *)pGMTTime);

	pTime->Year		= plocalTime->tm_year+1900;
	pTime->Month		= plocalTime->tm_mon+1;
	pTime->Day			= plocalTime->tm_mday;
	pTime->Hour		= plocalTime->tm_hour;
	pTime->Minute		= plocalTime->tm_min;
	pTime->Second		= plocalTime->tm_sec;
	pTime->WeekDay	= plocalTime->tm_wday;
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_current_ticktime
 * Description	: get number of ticks from system start up
 * Input    	: none
 * Output   	: none
 * Return   	: ticks count
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_current_ticktime(void)
{
	return (UPI_int32)times(NULL);
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_ticks_persecond
 * Description	: get ticks per second
 * Input    	: none
 * Output   	: none
 * Return   	: ticks per second
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_ticks_persecond(void)
{
	 return (UPI_int32)sysconf(_SC_CLK_TCK);
}

/*-------------------------------------------------------------------------
 * Function 	: __UPI_os_timer_trigger_thread
 * Description	: internal timer trigger thread
 * Input    	: none
 * Output   	: none
 * Return   	: no return
 * ----------------------------------------------------------------------*/
static void *__UPI_os_timer_trigger_thread(void *pArg)
{
	UPI_int32	rc;
	UPI_OsTimerMsg	msg;
	UPI_int32	count;

	while(1)
	{
		rc = UPI_os_queue_receive(__OsTimer.Queue, &msg, sizeof(UPI_OsTimerMsg), &count);
		if(rc != E_UPI_NO_ERROR)
		{
			UPI_os_error_printf("__UPI_os_timer_trigger_thread() QUEUE RECEIVE ERROR (RC = 0x%x)\n", rc);
		}

		if(msg.pTimerFunc == NULL)
		{
			/* should not happen  */
			UPI_os_error_printf(C_UPI_OS_UNKNOWN_ERROR_MSG);
			UPI_os_error_printf("__UPI_os_timer_trigger_thread() FUNC IS NULL\n");
		}
		else if(count != sizeof(UPI_OsTimerMsg))
		{
			UPI_os_error_printf(C_UPI_OS_CRITICAL_ERROR_MSG);
			UPI_os_error_printf("__UPI_os_timer_trigger_thread() MSG RECEIVE ERROR\n");
		}
		else
		{
			msg.pTimerFunc(msg.pArg);
		}
	}

}

/*-------------------------------------------------------------------------
 * Function 	: __UPI_os_timer_poll_thread
 * Description	: internal timer poll thread
 * Input    	: none
 * Output   	: none
 * Return   	: no return
 * ----------------------------------------------------------------------*/
static void *__UPI_os_timer_poll_thread(void *pArg)
{
	UPI_int32		rc;
	UPI_uint32		i;
	UPI_OsSysTime	delta;
	UPI_uint32		interval = C_UPI_OS_TIMER_POLL_INTERVAL;
	struct timeval	curtime, lasttime;
	struct timezone	tz;
	UPI_OsTimerMsg	msg;

	gettimeofday(&lasttime, &tz);
	lasttime.tv_sec	+= tz.tz_minuteswest*60;
	while(1)
	{
		gettimeofday(&curtime, &tz);
		curtime.tv_sec += tz.tz_minuteswest*60;

		if(curtime.tv_usec < lasttime.tv_usec)
		{
			delta.Milliseconds	=	1000000 + curtime.tv_usec - lasttime.tv_usec;
			delta.Seconds		=	curtime.tv_sec - lasttime.tv_sec - 1;
		}
		else
		{
			delta.Milliseconds	=	curtime.tv_usec - lasttime.tv_usec;
			delta.Seconds		=	curtime.tv_sec - lasttime.tv_sec;
		}

		if(delta.Seconds > 10 || delta.Seconds < 0)
		{
			delta.Seconds		=	1;
			delta.Milliseconds	=	0;
		}

//		printf("[%s] delta time is %us %06uus, curtime is %us %06uus\n", __FUNCTION__, delta.Seconds, delta.Milliseconds, curtime.tv_sec, curtime.tv_usec);

		lasttime.tv_sec		=	curtime.tv_sec;
		lasttime.tv_usec	=	curtime.tv_usec;

		rc = UPI_os_semaphore_wait(__OsTimer.Sema);
		if(rc != E_UPI_NO_ERROR)
		{
			continue;
		}

		for(i = 0; i < C_UPI_OS_MAX_TIMER_COUNT; i++)
		{
			if(__OsTimer.pTimer[i].Status == C_UPI_OS_TIMER_RUN)
			{
//				printf("[%s] Timer %d CurTrigger time is %ds %06uus\n", __FUNCTION__, i, __OsTimer.pTimer[i].NextTrigger.Seconds, __OsTimer.pTimer[i].NextTrigger.Milliseconds);
				if(__OsTimer.pTimer[i].NextTrigger.Milliseconds < delta.Milliseconds)
				{
					__OsTimer.pTimer[i].NextTrigger.Milliseconds	= 1000000 + __OsTimer.pTimer[i].NextTrigger.Milliseconds - delta.Milliseconds;
					__OsTimer.pTimer[i].NextTrigger.Seconds 		= __OsTimer.pTimer[i].NextTrigger.Seconds - 1 - delta.Seconds;
				}
				else
				{
					__OsTimer.pTimer[i].NextTrigger.Milliseconds	-= delta.Milliseconds;
					__OsTimer.pTimer[i].NextTrigger.Seconds 		-= delta.Seconds;
				}
//				printf("[%s] Timer %d NextTrigger time is %ds %06uus\n", __FUNCTION__, i, __OsTimer.pTimer[i].NextTrigger.Seconds, __OsTimer.pTimer[i].NextTrigger.Milliseconds);
				if(__OsTimer.pTimer[i].NextTrigger.Seconds <= 0)
				{
//					printf("[%s] Timer %d triggered ,current time is %u s\n", __FUNCTION__, i, curtime.tv_sec);
					msg.pTimerFunc = __OsTimer.pTimer[i].pTimerFunc;
					msg.pArg =  __OsTimer.pTimer[i].pArg;
					rc = UPI_os_queue_send(__OsTimer.Queue, &msg, sizeof(UPI_OsTimerMsg));
					if(rc != E_UPI_NO_ERROR)
					{
						UPI_os_error_printf(C_UPI_OS_CRITICAL_ERROR_MSG);
						UPI_os_error_printf("__UPI_os_timer_poll_thread() QUEUE SEND ERROR (RC = 0x%x)\n", rc);
					}

					if(__OsTimer.pTimer[i].Modulus.Seconds > 0)
					{
						__OsTimer.pTimer[i].NextTrigger.Seconds = __OsTimer.pTimer[i].Modulus.Seconds;
					}
					else
					{
//						memset(&(__OsTimer.pTimer[i]), 0, sizeof(UPI_OsTimerInfo));
						__OsTimer.pTimer[i].Status = C_UPI_OS_TIMER_STOP;
						break;
					}
				}
			}
		}

		rc = UPI_os_semaphore_post(__OsTimer.Sema);
		if(rc != E_UPI_NO_ERROR)
		{
			UPI_os_error_printf(C_UPI_OS_FATAL_ERROR_MSG);
			UPI_os_error_printf("__UPI_os_timer_poll_thread() SEMA POST ERROR (RC = 0x%x)\n", rc);
		}
		rc = UPI_os_thread_sleep(interval);
	}
}

/*-------------------------------------------------------------------------
 * Function 	: __UPI_os_timer_init
 * Description	: initialize internal timer data
 * Input    	: none
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
static UPI_int32 __UPI_os_timer_init()
{
	UPI_int32	rc;

	if(__OsTimer.InitFlag == 0)
	{
		rc = UPI_os_semaphore_create(&(__OsTimer.Sema) , 1);
		if(rc != E_UPI_NO_ERROR)
		{
			return E_UPI_OS_TIMER_INIT_ERROR;
		}

		rc = UPI_os_queue_create(&(__OsTimer.Queue) , 64, sizeof(UPI_OsTimerMsg));
		if(rc != E_UPI_NO_ERROR)
		{
			UPI_os_semaphore_destroy(&__OsTimer.Sema);
			return E_UPI_OS_TIMER_INIT_ERROR;
		}

		rc = UPI_os_thread_create(	__UPI_os_timer_poll_thread,
									NULL,
									C_UPI_OS_TIMER_POLL_THREAD_STACK_SIZE,
									C_UPI_OS_TIMER_POLL_THREAD_PRIORITY,
									C_UPI_OS_TIMER_POLL_THREAD_NAME,
									&(__OsTimer.TimerPollThread),
									C_UPI_OS_THREAD_CREATE_DEFAULT);
		if(rc != E_UPI_NO_ERROR)
		{
			UPI_os_queue_destroy(__OsTimer.Queue);
			UPI_os_semaphore_destroy(&__OsTimer.Sema);
			return E_UPI_OS_TIMER_INIT_ERROR;
		}

		rc = UPI_os_thread_create(	__UPI_os_timer_trigger_thread,
									NULL,
									C_UPI_OS_TIMER_TRIGGER_THREAD_STACK_SIZE,
									C_UPI_OS_TIMER_TRIGGER_THREAD_PRIORITY,
									C_UPI_OS_TIMER_TRIGGER_THREAD_NAME,
									&(__OsTimer.TimerTriggerThread),
									C_UPI_OS_THREAD_CREATE_DEFAULT);
		if(rc != E_UPI_NO_ERROR)
		{
			UPI_os_thread_destroy(__OsTimer.TimerPollThread);
			UPI_os_queue_destroy(__OsTimer.Queue);
			UPI_os_semaphore_destroy(&__OsTimer.Sema);
			return E_UPI_OS_TIMER_INIT_ERROR;
		}

		memset(__OsTimer.pTimer, 0, sizeof(__Timer));
		__OsTimer.InitFlag = 1;
	}

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_timer_create
 * Description	: create a timer
 * Input    	: none
 * Output   	: pTimerHandle receive timer handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_timer_create(pUPI_OsTimerHandle pTimerHandle)
{
	UPI_int32	rc;
	UPI_int32	i;

	rc = __UPI_os_timer_init();
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_create() init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_INIT_ERROR;
	}

	if(pTimerHandle == NULL)
	{
		UPI_os_error_printf("UPI_os_timer_create() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(__OsTimer.Sema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_create() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_CREATE_ERROR;
	}

	for(i = 0; i < C_UPI_OS_MAX_TIMER_COUNT; i++)
	{
		if(__OsTimer.pTimer[i].Status == C_UPI_OS_TIMER_NOT_USED)
		{
			*pTimerHandle = i;
			memset(&(__OsTimer.pTimer[i]), 0, sizeof(UPI_OsTimerInfo));
			__OsTimer.pTimer[i].Status = C_UPI_OS_TIMER_STOP;

			UPI_os_semaphore_post(__OsTimer.Sema);

			return E_UPI_NO_ERROR;
		}
	}

	UPI_os_semaphore_post(__OsTimer.Sema);

	UPI_os_error_printf("UPI_os_timer_create() no more timer\n");
	return E_UPI_OS_TIMER_CREATE_ERROR;

}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_timer_destroy
 * Description	: destroy a timer
 * Input    	: timer handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_timer_destroy(UPI_OsTimerHandle TimerHandle)
{
	UPI_int32	rc;

	rc = __UPI_os_timer_init();
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_destroy() init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_INIT_ERROR;
	}

	if(	TimerHandle < 0 ||
		TimerHandle >= C_UPI_OS_MAX_TIMER_COUNT ||
		__OsTimer.pTimer[TimerHandle].Status == C_UPI_OS_TIMER_NOT_USED)
	{
		UPI_os_error_printf("UPI_os_timer_destroy() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = UPI_os_semaphore_wait(__OsTimer.Sema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_destroy() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_DESTROY_ERROR;	
	}

	memset(&(__OsTimer.pTimer[TimerHandle]), 0, sizeof(UPI_OsTimerInfo));
	UPI_os_semaphore_post(__OsTimer.Sema);

	return E_UPI_NO_ERROR;

}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_timer_start
 * Description	: start a timer
 * Input    	: timer handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_timer_start(UPI_OsTimerHandle TimerHandle)
{
	UPI_int32	rc;

	rc = __UPI_os_timer_init();
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_start() init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_INIT_ERROR;
	}

	if(	TimerHandle < 0 ||
		TimerHandle >= C_UPI_OS_MAX_TIMER_COUNT ||
		__OsTimer.pTimer[TimerHandle].Status == C_UPI_OS_TIMER_NOT_USED)
	{
		UPI_os_error_printf("UPI_os_timer_start() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = UPI_os_semaphore_wait(__OsTimer.Sema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_start() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_START_ERROR;
	}

	__OsTimer.pTimer[TimerHandle].Status = C_UPI_OS_TIMER_RUN;
	UPI_os_semaphore_post(__OsTimer.Sema);

	return E_UPI_NO_ERROR;

}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_timer_stop
 * Description	: stop a timer
 * Input    	: timer handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_timer_stop(UPI_OsTimerHandle TimerHandle)
{
	UPI_int32	rc;

	rc = __UPI_os_timer_init();
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_stop() init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_INIT_ERROR;
	}

	if(	TimerHandle < 0 ||
		TimerHandle >= C_UPI_OS_MAX_TIMER_COUNT ||
		__OsTimer.pTimer[TimerHandle].Status == C_UPI_OS_TIMER_NOT_USED)
	{
		UPI_os_error_printf("UPI_os_timer_stop() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = UPI_os_semaphore_wait(__OsTimer.Sema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_stop() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_STOP_ERROR;
	}

	__OsTimer.pTimer[TimerHandle].Status = C_UPI_OS_TIMER_STOP;
	UPI_os_semaphore_post(__OsTimer.Sema);

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_timer_setup
 * Description	: setup a timer
 * Input    	: timer handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_timer_setup(UPI_OsTimerHandle TimerHandle,
									void (*pTimerFunc)(void*),
									void *pArg,
									pUPI_OsSysTime pInitValue,
									pUPI_OsSysTime pModulus)
{
	UPI_int32	rc;

	rc = __UPI_os_timer_init();
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_setup() init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_INIT_ERROR;
	}

	if(	TimerHandle < 0 ||
		TimerHandle >= C_UPI_OS_MAX_TIMER_COUNT ||
		__OsTimer.pTimer[TimerHandle].Status == C_UPI_OS_TIMER_NOT_USED)
	{
		UPI_os_error_printf("UPI_os_timer_setup() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	if( pTimerFunc == NULL ||
		pInitValue == NULL ||
		pInitValue->Milliseconds != 0 ||
		pModulus == NULL ||
		pModulus->Milliseconds != 0 )
	{
		UPI_os_error_printf("UPI_os_timer_setup() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(__OsTimer.Sema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_timer_setup() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_TIMER_SETUP_ERROR;
	}

	__OsTimer.pTimer[TimerHandle].pTimerFunc	= pTimerFunc;
	__OsTimer.pTimer[TimerHandle].pArg			= pArg;
	memcpy(&(__OsTimer.pTimer[TimerHandle].InitValue), pInitValue, sizeof(UPI_OsSysTime));
	memcpy(&(__OsTimer.pTimer[TimerHandle].Modulus), pModulus, sizeof(UPI_OsSysTime));
	memcpy(&(__OsTimer.pTimer[TimerHandle].NextTrigger), pInitValue, sizeof(UPI_OsSysTime));

	UPI_os_semaphore_post(__OsTimer.Sema);

	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_create
 * Description	: create a thread
 * Input    	: pStartFunc	thread routine
 				  pArg			pointer to argument which is pass to pStartFunc
 				  StackSize		stack size
 				  Priority		thread priority
 				  pName			pointer to thread name
 				  CreateFlag	thread create feature
 * Output   	: pThreadHandle	receive thread handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_create(	void *(*pStartFunc)(void *),
								void *pArg,
								UPI_uint32 StackSize,
								UPI_OsPriority Priority,
								UPI_int8 *pName,
								pUPI_OsThreadHandle pThreadHandle,
								UPI_OsThreadCreateFlag	CreateFlag)
{

	UPI_int32 rc;
	pthread_attr_t attr;
	struct sched_param param;

	if(	pStartFunc == NULL ||
		pThreadHandle == NULL ||
		Priority > C_UPI_OS_MAX_THREAD_PRIORITY)
	{
		UPI_os_error_printf("UPI_os_thread_create() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	if(CreateFlag != C_UPI_OS_THREAD_CREATE_DEFAULT)
	{
		UPI_os_error_printf("UPI_os_thread_create() feature not support\n");
		return E_UPI_OS_THREAD_FEATURE_NOT_SUPPORT;
	}

	rc = pthread_attr_init(&attr);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_create() attr init error (rc = 0x%x)\n", rc);
		return E_UPI_OS_THREAD_CREATE_ERROR;
	}

	if(StackSize == 0 || StackSize < C_UPI_OS_MIN_STACKSIZE)
	{
		StackSize = C_UPI_OS_DEFAULT_STACKSIZE;
	}
	else if(StackSize > C_UPI_OS_MAX_STACKSIZE)
	{
		UPI_os_error_printf("UPI_os_thread_create() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = pthread_attr_setstacksize(&attr, StackSize);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_create() attr setstacksize error (rc = 0x%x)\n", rc);
		pthread_attr_destroy(&attr);
		return E_UPI_OS_THREAD_CREATE_ERROR;
	}
	pthread_attr_setschedpolicy(&attr, SCHED_RR);
	param.sched_priority = Priority + 1;
	pthread_attr_setschedparam(&attr, &param);

	rc = pthread_create((pthread_t*)pThreadHandle, &attr, pStartFunc, pArg);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_create() error (rc = 0x%x)\n", rc);
		pthread_attr_destroy(&attr);
		return E_UPI_OS_THREAD_CREATE_ERROR;
	}
//	pthread_attr_destroy(&attr);
	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_get_handle
 * Description	: get current thread handle
 * Input    	: none
 * Output   	: pThreadHandle	receive current thread handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_get_handle(pUPI_OsThreadHandle pThreadHandle)
{
	if(pThreadHandle == NULL)
	{
		UPI_os_error_printf("UPI_os_thread_get_handle() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	*pThreadHandle = pthread_self();
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_get_priority
 * Description	: get thread priority
 * Input    	: ThreadHandle	thread handle
 * Output   	: pPriority receive priority
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_get_priority(UPI_OsThreadHandle	ThreadHandle, pUPI_OsPriority pPriority)
{
	UPI_int32 rc;
	UPI_int32 Policy;
	struct sched_param param;

	if(pPriority == NULL)
	{
		UPI_os_error_printf("UPI_os_thread_get_priority() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = pthread_getschedparam(ThreadHandle, &Policy, &param);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_get_priority() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_THREAD_GET_PRIORITY_ERROR;
	}

	*pPriority = param.sched_priority - 1;
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_set_priority
 * Description	: set thread handle
 * Input    	: ThreadHandle	thread handle
 				  Priority		new priority
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_set_priority(UPI_OsThreadHandle	ThreadHandle, UPI_OsPriority Priority)
{
	UPI_int32 rc;
	struct sched_param param;

	if(	Priority > C_UPI_OS_MAX_THREAD_PRIORITY)
	{
		UPI_os_error_printf("UPI_os_thread_set_priority() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	param.sched_priority = Priority + 1;

	rc = pthread_setschedparam(ThreadHandle, SCHED_OTHER, &param);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_set_priority() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_THREAD_SET_PRIORITY_ERROR;
	}

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_destroy
 * Description	: destroy a thread
 * Input    	: ThreadHandle thread handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_destroy(UPI_OsThreadHandle ThreadHandle)
{
	UPI_int32	rc;
	void *res = NULL;	
#if 0
	rc = pthread_cancel(ThreadHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_thread_destroy() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_THREAD_DESTROY_ERROR;
	}
	else
	{
		return E_UPI_NO_ERROR;
	}
#endif

	rc = pthread_join((pthread_t)ThreadHandle, &res);	
	if (rc != 0)
	{
		printf("pthread_join\n");	
	}

	if (res == 0)	
	{		
		printf("thread was deleted\n");		
		return E_UPI_NO_ERROR;	
	}	
	else	
	{		
		printf("thread wasn't deleted (shouldn't happen!)\n");		
		return E_UPI_FALSE;	
	}

}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_thread_sleep
 * Description	: thread sleep a while
 * Input    	: ms	sleep time(ms)
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_thread_sleep(UPI_uint32 ms)
{
	usleep(ms * 1000);
	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_semaphore_create
 * Description	: create a semaphore
 * Input    	: InitValue	initialized value
 * Output   	: pSemHanle receive semaphore handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_semaphore_create(pUPI_OsSemHandle pSemHandle, UPI_uint32 InitValue)
{
	UPI_int32	rc;
	void		*ptr = NULL;

	if(pSemHandle == NULL)
	{
		UPI_os_error_printf("UPI_os_semaphore_create() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	ptr = UPI_os_malloc((UPI_uint32)sizeof(sem_t));
	if(ptr == NULL)
	{
		UPI_os_error_printf("UPI_os_semaphore_create() not enough memory\n");
		return E_UPI_NOT_ENOUGH_MEMORY;
	}
	*pSemHandle = (UPI_OsSemHandle)ptr;
	memset((void *)*pSemHandle, 0, sizeof(sem_t));
	rc = sem_init((sem_t *)(*pSemHandle), 0, InitValue);
	if(rc != 0)
	{
		UPI_os_free(ptr);
		*pSemHandle = (UPI_OsSemHandle)C_UPI_OS_INVALID_SEMA_HANDLE;
		UPI_os_error_printf("UPI_os_semaphore_create() error\n");
		return E_UPI_OS_SEMA_CREATE_ERROR;
	}

	return E_UPI_NO_ERROR;

}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_semaphore_destroy
 * Description	: destroy a semaphore
 * Input    	: SemHandle	semaphore handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_semaphore_destroy(pUPI_OsSemHandle pSemHandle)
{
	UPI_int32	rc;
	sem_t		*ptr;

	if(C_UPI_OS_INVALID_SEMA_HANDLE ==(pUPI_OsSemHandle)(*pSemHandle)|| pSemHandle == (pUPI_OsSemHandle)NULL)
	{
		UPI_os_error_printf("UPI_os_semaphore_destroy() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	ptr = (sem_t*)(*pSemHandle);
	rc = sem_destroy(ptr);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_semaphore_destroy() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_SEMA_DESTROY_ERROR;
	}
	UPI_os_free(ptr);
	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_semaphore_wait
 * Description	: wait a semaphore
 * Input    	: SemHandle semaphore handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_semaphore_wait(UPI_OsSemHandle SemHandle)
{
	UPI_int32	rc;

	if(SemHandle == (UPI_OsSemHandle)C_UPI_OS_INVALID_SEMA_HANDLE)
	{
		UPI_os_error_printf("UPI_os_semaphore_wait() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	while((rc = sem_wait((sem_t *)SemHandle))!=0)
	{
		if((errno!=EINTR)&&(rc!=0))
		{
			UPI_os_error_printf("UPI_os_semaphore_wait(0x%x) error is: %s (rc = 0x%x)\n", SemHandle, strerror(errno), rc);
			return E_UPI_OS_SEMA_WAIT_ERROR;
		}
	}

	#if 0
	rc = sem_wait((sem_t *)SemHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_semaphore_wait(0x%x) error is: %s (rc = 0x%x)\n", SemHandle, strerror(errno), rc);
		return E_UPI_OS_SEMA_WAIT_ERROR;
	}
	#endif

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_semaphore_timed_wait
 * Description	: wait a semaphore with timeout
 * Input    	: SemHandle semaphore handle
 				  msTime	timeout time
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_semaphore_timed_wait(UPI_OsSemHandle SemHandle, UPI_uint32 msTime)
{
	UPI_int32	rc;
	UPI_int32	loop, interval = 10;

	if(SemHandle == (UPI_OsSemHandle)C_UPI_OS_INVALID_SEMA_HANDLE)
	{
		UPI_os_error_printf("UPI_os_semaphore_timed_wait() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

/*	if(	msTime < 0 )
	{
		UPI_os_error_printf("UPI_os_semaphore_timed_wait() param error\n");
		return E_UPI_PARAM_ERROR;
	}*/

	loop = msTime / interval;

	while(1)
	{
		rc = sem_trywait((sem_t *)SemHandle);
		if(rc == 0)
		{
			return E_UPI_NO_ERROR;
		}
		else
		{
			loop--;
			if(loop > 0)
			{
				UPI_os_thread_sleep(interval);
				continue;
			}
			else
			{
//				UPI_os_error_printf("UPI_os_semaphore_timed_wait() timeout\n");
				return E_UPI_OS_SEMA_TWAIT_TIMEOUT;
			}
		}
	}
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_semaphore_post
 * Description	: post a semaphore
 * Input    	: SemHandle semaphore handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_semaphore_post(UPI_OsSemHandle SemHandle)
{
	UPI_int32	rc;

	if(SemHandle == (UPI_OsSemHandle)C_UPI_OS_INVALID_SEMA_HANDLE)
	{
		UPI_os_error_printf("UPI_os_semaphore_post() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = sem_post((sem_t *)SemHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_semaphore_post() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_SEMA_POST_ERROR;
	}

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_create
 * Description	: create a message queue
 * Input    	: MaxMessage		max message count
 				  MaxMessageSize	max message size
 * Output   	: pQueueHandle		receive queue handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_create(	pUPI_OsQueueHandle pQueueHandle,
								UPI_uint32 MaxMessage,
								UPI_uint32 MaxMessageSize)
{
	UPI_int32	rc;

	pUPI_OsQueueInfo *ppQueueInfo = (pUPI_OsQueueInfo *)pQueueHandle;

	if( ppQueueInfo == NULL ||
		MaxMessage == 0 ||
		MaxMessage > C_UPI_OS_MAX_MSG ||
		MaxMessageSize == 0 ||
		MaxMessageSize > C_UPI_OS_MAX_MSG_SIZE)
	{
		UPI_os_error_printf("[%s] param error\n", __FUNCTION__);
		return E_UPI_PARAM_ERROR;
	}

	*ppQueueInfo = UPI_os_malloc(sizeof(UPI_OsQueueInfo));
	if(*ppQueueInfo == NULL)
	{
		UPI_os_error_printf("[%s] not enough memory\n", __FUNCTION__);
		return E_UPI_NOT_ENOUGH_MEMORY;
	}

	memset(*ppQueueInfo, 0, sizeof(UPI_OsQueueInfo));

	(*ppQueueInfo)->MaxMessage		= MaxMessage;
	(*ppQueueInfo)->MaxMessageSize	= MaxMessageSize;

	(*ppQueueInfo)->pMsgPool = UPI_os_malloc((MaxMessageSize + sizeof(UPI_int32))  * MaxMessage);
	if((*ppQueueInfo)->pMsgPool == NULL)
	{
		UPI_os_free(*ppQueueInfo);
		*ppQueueInfo = NULL;
		UPI_os_error_printf("UPI_os_queue_create() not enough memory\n");
		return E_UPI_NOT_ENOUGH_MEMORY;
	}

	memset((*ppQueueInfo)->pMsgPool, 0, (MaxMessageSize + sizeof(UPI_int32))  * MaxMessage);

	rc = UPI_os_semaphore_create(&((*ppQueueInfo)->ProtectSema), 1);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_free((*ppQueueInfo)->pMsgPool);
		UPI_os_free(*ppQueueInfo);
		*ppQueueInfo = NULL;
		UPI_os_error_printf("UPI_os_queue_create() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_CREATE_ERROR;
	}

	rc = UPI_os_semaphore_create(&((*ppQueueInfo)->SyncSema), 0);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_semaphore_destroy(&((*ppQueueInfo)->ProtectSema));
		UPI_os_free((*ppQueueInfo)->pMsgPool);
		UPI_os_free(*ppQueueInfo);
		*ppQueueInfo = NULL;
		UPI_os_error_printf("UPI_os_queue_create() error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_CREATE_ERROR;
	}

	(*ppQueueInfo)->Sig = C_UPI_OS_QUEUE_SIGNATURE;
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_destroy
 * Description	: destroy a message queue
 * Input    	: QueueHandle	queue handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_destroy(UPI_OsQueueHandle QueueHandle)
{
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}
	UPI_os_semaphore_destroy(&(pQueueInfo->SyncSema));
	UPI_os_semaphore_destroy(&(pQueueInfo->ProtectSema));
	UPI_os_free(pQueueInfo->pMsgPool);
	UPI_os_free(pQueueInfo);

	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_send
 * Description	: send a message to queue
 * Input    	: QueueHandle	queue handle
 				  pBuffer		message buffer
 				  BufferSize	message size
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_send(	UPI_OsQueueHandle QueueHandle,
								void* pBuffer,
								UPI_uint32 BufferSize)
{
	UPI_int32	rc;
	UPI_int32	Index;
	UPI_int8	*pMsg;
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(	pBuffer == NULL || BufferSize == 0)
	{
		UPI_os_error_printf("UPI_os_queue_send() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_queue_send() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_SEND_ERROR;
	}

	if(pQueueInfo->MessageCount >= pQueueInfo->MaxMessage)
	{
		UPI_os_semaphore_post(pQueueInfo->ProtectSema);
		UPI_os_error_printf("UPI_os_queue_send() queue full\n");
		return E_UPI_OS_QUEUE_FULL_ERROR;
	}

	if(BufferSize > pQueueInfo->MaxMessageSize)
	{
		BufferSize = pQueueInfo->MaxMessageSize;
	}

	Index = (pQueueInfo->Index + pQueueInfo->MessageCount) % pQueueInfo->MaxMessage;
	pMsg = pQueueInfo->pMsgPool + Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));

	memset(pMsg, 0, pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memcpy(pMsg, &BufferSize, sizeof(UPI_int32));
	memcpy(pMsg + sizeof(UPI_int32), pBuffer, BufferSize);

	pQueueInfo->MessageCount++;
	UPI_os_semaphore_post(pQueueInfo->ProtectSema);

	UPI_os_semaphore_post(pQueueInfo->SyncSema);

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Input    	: QueueHandle	queue handle
 				  pBuffer		message buffer
 				  BufferSize	message size
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_send_unique_type(	UPI_OsQueueHandle QueueHandle,
											void* pBuffer,
											UPI_uint32 BufferSize)
{
	UPI_int32	rc;
	UPI_int32	Index, i = 0;
	UPI_int8	*pMsg;
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;
	UPI_int32	buffersize;

	UPI_int32	inbufferaddr;
	UPI_int32	*ptype = pBuffer;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(	pBuffer == NULL || BufferSize == 0)
	{
		UPI_os_error_printf("UPI_os_queue_send() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_queue_send() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_SEND_ERROR;
	}

//	printf("[%s] start handle %x current message count %d input type is %x\n", __FUNCTION__, QueueHandle, pQueueInfo->MessageCount, *ptype);

	if(BufferSize > pQueueInfo->MaxMessageSize)
	{
		buffersize = pQueueInfo->MaxMessageSize;
	}
	else
		buffersize = BufferSize;

	for(i = 0; i < pQueueInfo->MessageCount; i++)
	{
//		printf("[%s] i %d\n", __FUNCTION__, i);
		Index = (pQueueInfo->Index + i) % pQueueInfo->MaxMessage;
//		printf("[%s] index %d\n", __FUNCTION__, Index);
		pMsg = pQueueInfo->pMsgPool + Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
		ptype = (UPI_int32*)pMsg + sizeof(UPI_int32);
//		printf("[%s] i %d type %x\n", __FUNCTION__, i, *ptype);
		if(memcmp(pMsg + sizeof(UPI_int32), (UPI_int8*)pBuffer, 4) == 0)
			break;
	}

	if(i >= pQueueInfo->MaxMessage)
	{
		UPI_os_semaphore_post(pQueueInfo->ProtectSema);
		UPI_os_error_printf("UPI_os_queue_send() queue full\n");
		return E_UPI_OS_QUEUE_FULL_ERROR;
	}

//	printf("[%s] find i is %d\n", __FUNCTION__, i);
	Index = (pQueueInfo->Index + i) % pQueueInfo->MaxMessage;
	pMsg = pQueueInfo->pMsgPool + Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));

	if(i < pQueueInfo->MessageCount)
	{
		memcpy(&inbufferaddr, pMsg + sizeof(UPI_int32) + sizeof(UPI_int32), 4);
		if(inbufferaddr)
		{
//			printf("[%s] free %x\n", __FUNCTION__, inbufferaddr);
			free((void*)inbufferaddr);
		}
	}

	memset(pMsg, 0, pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memcpy(pMsg, &buffersize, sizeof(UPI_int32));
	memcpy(pMsg + sizeof(UPI_int32), pBuffer, buffersize);

	if(i == pQueueInfo->MessageCount)
	{
		pQueueInfo->MessageCount++;
		UPI_os_semaphore_post(pQueueInfo->SyncSema);
	}

	UPI_os_semaphore_post(pQueueInfo->ProtectSema);

	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_send_lifo
 * Description	: send a message to queue, last in first out
 * Input    	: QueueHandle	queue handle
 				  pBuffer		message buffer
 				  BufferSize	message size
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_send_lifo(UPI_OsQueueHandle QueueHandle,
								void* pBuffer,
								UPI_uint32 BufferSize)
{
	UPI_int32	rc;
	UPI_int32	Index;
	UPI_int8	*pMsg;
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(	pBuffer == NULL || BufferSize == 0)
	{
		UPI_os_error_printf("UPI_os_queue_send() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_queue_send() sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_SEND_ERROR;
	}

	if(pQueueInfo->MessageCount >= pQueueInfo->MaxMessage)
	{
		UPI_os_semaphore_post(pQueueInfo->ProtectSema);
		UPI_os_error_printf("UPI_os_queue_send() queue full\n");
		return E_UPI_OS_QUEUE_FULL_ERROR;
	}

	if(BufferSize > pQueueInfo->MaxMessageSize)
	{
		BufferSize = pQueueInfo->MaxMessageSize;
	}

	if(pQueueInfo->MessageCount == 0)
	{
		Index = (pQueueInfo->Index + pQueueInfo->MessageCount) % pQueueInfo->MaxMessage;
	}
	else
	{
		pQueueInfo->Index = (pQueueInfo->Index - 1 + pQueueInfo->MaxMessage) % pQueueInfo->MaxMessage;
		Index = pQueueInfo->Index;
	}

	pMsg = pQueueInfo->pMsgPool + Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memset(pMsg, 0, pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memcpy(pMsg, &BufferSize, sizeof(UPI_int32));
	memcpy(pMsg + sizeof(UPI_int32), pBuffer, BufferSize);

	pQueueInfo->MessageCount++;
	UPI_os_semaphore_post(pQueueInfo->ProtectSema);

	UPI_os_semaphore_post(pQueueInfo->SyncSema);

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_receive
 * Description	: receive message from queue
 * Input    	: QueueHandle	queue handle
 				  BufferSize	size of buffer
 * Output   	: pBuffer		buffer to receive message
 				  pCount		number of byte received
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_receive(	UPI_OsQueueHandle QueueHandle,
									void* pBuffer,
									UPI_uint32 BufferSize,
									UPI_int32 *pCount )
{
	UPI_int32	rc;
	UPI_int32	Size;
	UPI_int8	*pMsg;
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		LOGI("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		LOGI("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(	pBuffer == NULL ||
		BufferSize == 0)
	{
		LOGI("UPI_os_queue_receive() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->SyncSema);
	if(rc != E_UPI_NO_ERROR)
	{
		LOGI("UPI_os_queue_receive() sync sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_RECEIVE_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	if(rc != E_UPI_NO_ERROR)
	{
		LOGI("UPI_os_queue_receive() protect sema wait error (rc = 0x%x)\n", rc);
		return E_UPI_OS_QUEUE_RECEIVE_ERROR;
	}

	if( pQueueInfo->Sig == C_UPI_OS_QUEUE_CLEARED )
	{
		pQueueInfo->Sig = C_UPI_OS_QUEUE_SIGNATURE;
		UPI_os_warn_printf("[%s] queue cleared.\n", __FUNCTION__);
		//UPI_os_semaphore_post(pQueueInfo->ProtectSema);
		LOGI("**************%s,%d***************\n",__FUNCTION__, __LINE__);
		//return E_UPI_OS_QUEUE_CLEARED;
	}

	pMsg = pQueueInfo->pMsgPool + pQueueInfo->Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memcpy(&Size, pMsg, sizeof(UPI_int32));
	if(Size > BufferSize)
	{
		Size = BufferSize;
	}

	memcpy(pBuffer, pMsg + sizeof(UPI_int32), Size);
	*pCount = Size;

	pQueueInfo->Index++;
	pQueueInfo->Index = pQueueInfo->Index % pQueueInfo->MaxMessage;
	if(pQueueInfo->MessageCount == 0)
	{
		//should not happen
		UPI_os_error_printf(C_UPI_OS_FATAL_ERROR_MSG);
		UPI_os_error_printf("UPI_os_queue_receive() UNKNOWN ERROR handle %x\n", QueueHandle);
	}

	pQueueInfo->MessageCount--;
	UPI_os_semaphore_post(pQueueInfo->ProtectSema);
	LOGI("**************%s,%d***************\n",__FUNCTION__, __LINE__);	
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_queue_timed_receive
 * Description	: receive message from queue with timeout
 * Input    	: QueueHandle	queue handle
 				  BufferSize	size of buffer
 				  msTime		timeout time
 * Output   	: pBuffer		buffer to receive message
 				  pCount		number of byte received
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_queue_timed_receive(	UPI_OsQueueHandle QueueHandle,
											void* pBuffer,
											UPI_uint32 BufferSize,
											UPI_int32 *pCount,
											UPI_uint32 msTime)
{
	UPI_int32	rc;
	UPI_int32	Size;
	UPI_int8	*pMsg;
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(	pBuffer == NULL ||
		BufferSize == 0)
	{
		UPI_os_error_printf("UPI_os_queue_timed_receive() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	rc = UPI_os_semaphore_timed_wait(pQueueInfo->SyncSema, msTime);
	if(rc == E_UPI_OS_SEMA_TWAIT_TIMEOUT)
	{
//		UPI_os_error_printf("UPI_os_queue_timed_receive() timeout error\n");
		return E_UPI_OS_QUEUE_TRECEIVE_TIMEOUT;
	}
	else if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_queue_timed_receive() sync sema wait error\n");
		return E_UPI_OS_QUEUE_RECEIVE_ERROR;
	}

	rc = UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	if(rc != E_UPI_NO_ERROR)
	{
		UPI_os_error_printf("UPI_os_queue_timed_receive() protect sema wait error\n");
		return E_UPI_OS_QUEUE_RECEIVE_ERROR;
	}

	if( pQueueInfo->Sig == C_UPI_OS_QUEUE_CLEARED )
	{
		pQueueInfo->Sig = C_UPI_OS_QUEUE_SIGNATURE;
		UPI_os_warn_printf("[%s] queue cleared.\n", __FUNCTION__);
		//UPI_os_semaphore_post(pQueueInfo->ProtectSema);
		//return E_UPI_OS_QUEUE_CLEARED;
	}

	pMsg = pQueueInfo->pMsgPool + pQueueInfo->Index * (pQueueInfo->MaxMessageSize + sizeof(UPI_int32));
	memcpy(&Size, pMsg, sizeof(UPI_int32));
	if(Size > BufferSize)
	{
		Size = BufferSize;
	}

	memcpy(pBuffer, pMsg + sizeof(UPI_int32), Size);
	*pCount = Size;

	pQueueInfo->Index++;
	pQueueInfo->Index = pQueueInfo->Index % pQueueInfo->MaxMessage;
	if(pQueueInfo->MessageCount == 0)
	{
		//should not happen
		UPI_os_error_printf(C_UPI_OS_FATAL_ERROR_MSG);
		UPI_os_error_printf("UPI_os_queue_timed_receive() UNKNOWN ERROR\n");
	}

	pQueueInfo->MessageCount--;
	UPI_os_semaphore_post(pQueueInfo->ProtectSema);
	return E_UPI_NO_ERROR;
}


/*-------------------------------------------------------------------------
 * Function 	: UPI_os_quene_clear
 * Description	: clear message quene
 * Input    	: QueueHandle	quene handle
 * Output   	: none
 * Return   	:
 * ----------------------------------------------------------------------*/
UPI_int32	UPI_os_queue_clear(UPI_OsQueueHandle QueueHandle)
{
	pUPI_OsQueueInfo	pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}
	UPI_os_semaphore_wait(pQueueInfo->ProtectSema);
	//0
	memset(pQueueInfo->pMsgPool, 0, (pQueueInfo->MaxMessageSize + sizeof(UPI_int32)) * pQueueInfo->MaxMessage);
	printf("[%s] current message %d\n", __FUNCTION__, pQueueInfo->MessageCount);
	while(pQueueInfo->MessageCount)
	{
		UPI_os_semaphore_wait(pQueueInfo->SyncSema);//�������м�����Ϣ��SyncSema 0
		pQueueInfo->MessageCount--;
	}
	pQueueInfo->Sig = C_UPI_OS_QUEUE_CLEARED;
	pQueueInfo->Index = 0;

	UPI_os_semaphore_post(pQueueInfo->ProtectSema);
	// 1
	//UPI_os_semaphore_post(pQueueInfo->SyncSema);

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_quene_getbuffer
 * Description	: get message quene buffer and current message count
 * Input    	: QueueHandle	quene handle
 				: MessageCount	current message count
 * Output   	: none
 * Return   	:
 * ----------------------------------------------------------------------*/
UPI_int32	UPI_os_queue_getbuffer(UPI_OsQueueHandle QueueHandle, void** buffer, UPI_int32* MessageCount, UPI_OsSemHandle *pSemHandle)
{
	pUPI_OsQueueInfo pQueueInfo = (pUPI_OsQueueInfo)QueueHandle;

	if(pQueueInfo == C_UPI_OS_INVALID_QUEUE_HANDLE)
	{
		UPI_os_error_printf("[%s] invalid handle\n", __FUNCTION__);
		return E_UPI_INVALID_HANDLE;
	}

	if(pQueueInfo->Sig != C_UPI_OS_QUEUE_SIGNATURE && pQueueInfo->Sig != C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status unkown\n", __FUNCTION__);
		return E_UPI_UNKNOWN_ERROR;
	}

	if(pQueueInfo->Sig == C_UPI_OS_QUEUE_CLEARED)
	{
		UPI_os_error_printf("[%s] current status is cleaning\n", __FUNCTION__);
		return E_UPI_OS_QUEUE_CLEARED;
	}

	UPI_os_semaphore_wait(pQueueInfo->ProtectSema);

	*buffer = pQueueInfo->pMsgPool;
	*MessageCount = pQueueInfo->MessageCount;
	*pSemHandle = pQueueInfo->ProtectSema;
//	memset(pQueueInfo->pMsgPool, 0, (pQueueInfo->MaxMessageSize + sizeof(UPI_int32)) * pQueueInfo->MaxMessage);
//	pQueueInfo->MessageCount = 0;
//	pQueueInfo->Index = 0;
	UPI_os_semaphore_post(pQueueInfo->ProtectSema);

	return E_UPI_NO_ERROR;
}
/*-------------------------------------------------------------------------
 * Function 	: UPI_os_malloc
 * Description	: allocate memory
 * Input    	: Size	memory size to allocate
 * Output   	: none
 * Return   	: pointer to allocated memory
 * ----------------------------------------------------------------------*/
void *UPI_os_malloc(UPI_uint32 Size)
{
	void *pBuffer;

	pBuffer = malloc(Size);

	/* Memory useage logging */
	/* ... */


	if(pBuffer == NULL)
	{
		UPI_os_error_printf("UPI_os_malloc : No more memory!\n");
	}

	return pBuffer;
}

/*-------------------------------------------------------------------------
 * Function 	: _UPI_os_free
 * Description	: free a memory
 * Input    	: pBuffer	pointer to memory
 * Output   	: none
 * Return   	: none
 * ----------------------------------------------------------------------*/
void _UPI_os_free(void *pBuffer)
{
	if(pBuffer != NULL)
	{
		free(pBuffer);
		/* Memory useage logging */
		/* ... */

	}
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_mutex_create
 * Description	: create a mutex
 * Input    	:
 * Output   	: pMutexHandle receive mutex handle
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_mutex_create(pUPI_OsMutexHandle pMutexHandle)
{
	UPI_int32	rc;
	void		*ptr = NULL;

	if(pMutexHandle == NULL)
	{
		UPI_os_error_printf("UPI_os_mutex_create() param error\n");
		return E_UPI_PARAM_ERROR;
	}

	ptr = UPI_os_malloc(sizeof(pthread_mutex_t));
	if(ptr == NULL)
	{
		UPI_os_error_printf("UPI_os_mutex_create() not enough memory\n");
		return E_UPI_NOT_ENOUGH_MEMORY;
	}
	*pMutexHandle = (UPI_OsMutexHandle)ptr;
	memset((void *)*pMutexHandle, 0, sizeof(UPI_int32));

	rc = pthread_mutex_init((pthread_mutex_t *)*pMutexHandle, NULL);
	if(rc != 0)
	{
		UPI_os_free(ptr);
		*pMutexHandle = (UPI_OsMutexHandle)NULL;
		UPI_os_error_printf("UPI_os_mutex_create() error\n");
		return E_UPI_FALSE;
	}

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_mutex_destroy
 * Description	: destroy a mutex
 * Input    	: MutexHandle	mutex handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_mutex_destroy(UPI_OsMutexHandle MutexHandle)
{
	UPI_int32	rc;
	void		*temp;

	if(MutexHandle == (UPI_OsMutexHandle)NULL)
	{
		UPI_os_error_printf("UPI_os_mutex_destroy() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = pthread_mutex_destroy((pthread_mutex_t *)MutexHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_mutex_destroy() error (rc = 0x%x)\n", rc);
		return E_UPI_FALSE;
	}
	temp = (void*)MutexHandle;
	UPI_os_free(temp);
	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_mutex_lock
 * Description	: lock mutex
 * Input    	: MutexHandle mutex handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_mutex_lock(UPI_OsMutexHandle MutexHandle)
{
	UPI_int32	rc;

	if(MutexHandle == (UPI_OsMutexHandle)NULL)
	{
		UPI_os_error_printf("UPI_os_mutex_lock() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = pthread_mutex_lock((pthread_mutex_t *)MutexHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_mutex_lock() error (rc = 0x%x)\n", rc);
		return E_UPI_FALSE;
	}

	return E_UPI_NO_ERROR;
}

/*-------------------------------------------------------------------------
 * Function 	: UPI_os_mutex_unlock
 * Description	: unlock mutex
 * Input    	: MutexHandle mutex handle
 * Output   	: none
 * Return   	: error code
 * ----------------------------------------------------------------------*/
UPI_int32 UPI_os_mutex_unlock(UPI_OsMutexHandle MutexHandle)
{
	UPI_int32	rc;

	if(MutexHandle == (UPI_OsMutexHandle)NULL)
	{
		UPI_os_error_printf("UPI_os_mutex_unlock() invalid handle\n");
		return E_UPI_INVALID_HANDLE;
	}

	rc = pthread_mutex_unlock((pthread_mutex_t *)MutexHandle);
	if(rc != 0)
	{
		UPI_os_error_printf("UPI_os_mutex_unlock() error (rc = 0x%x)\n", rc);
		return E_UPI_FALSE;
	}

	return E_UPI_NO_ERROR;
}
